/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*#include "filter.h"
#include "main.h"       not needed with celpfilt.h */
#include "celpfilt.h"
#ifdef FLOATING_POINT
#include "showdata.h"
#endif
#include "adapt.h"
#include "lp_anal.h"
#include "bwexp.h"
#include "analysis.h"
#include "con_adap.h"
#include "interp.h"
#include "lsftopc.h"
#include "mexcite.h"
#include "stoch.h"
#include "zero_array16.h"
#include "move_array16.h"

STATIC void CreateSubFrame(
fxpt_16 FutureSpeech[F_LEN],
fxpt_16 PresentSpeech[F_LEN]);

STATIC void FindImpulseResponse(
fxpt_16 pc[ORDER+1],
fxpt_16 h[SF_LEN]);

STATIC void FindLPResidual(
fxpt_16 speech[SF_LEN],
fxpt_16 pc[ORDER+1],
fxpt_16 residual[RES_LEN]);

STATIC void UpdateEverything(
fxpt_16 OptExcVec[SF_LEN],
fxpt_16 Speech[SF_LEN],
fxpt_16 AdaptCB[ACB_SIZE],
fxpt_16 AdaptGain,
fxpt_16 AdaptDelay,
fxpt_16 pc[ORDER+1]);

/**************************************************************************
*                                                                         *
* ROUTINE
*               analysis
*
* FUNCTION
*               CELP analysis
* SYNOPSIS
*               analysis(SpeechIn, parameters, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       SpeechIn        fxpt_16  i      Frame of input speech
*       parameters      TX_PARAM o      Frame of CELP parameters
*       frame_num       int      i      Frame number (for diagnostics)
*
**************************************************************************
*
* DESCRIPTION
*
*       This performs the analysis of the input speech and produces
*       a set of parameters that are sent to the synthesizer for
*       reconstruction.
*
**************************************************************************
*
* CALLED BY
*
*       celp.c
*
* CALLS
*
*       HPF_InSpeech LP_Analysis Interpolate CreateSubFrame
*
*       loop around:
*       LSFtoPC FindLPResidual FindImpulseResponse AdaptiveAnalysis
*       ModifiedExcitation StochasticAnalysis UpdateEverything
*
**************************************************************************/
void Analysis(
fxpt_16 SpeechIn[F_LEN],
TX_PARAM *parameters
)
{
        fxpt_16         LSFint[NUM_SF][ORDER];          /* 0.15 format */
        fxpt_16         pc[ORDER+1];                    /* 2.13 format */
        int             sf;
        fxpt_16         LP_res[RES_LEN];                /* 15.0 format */
        fxpt_16         Adaptive_res[RES_LEN];          /* 15.0 format */
        fxpt_16         LPImpulseResponse[SF_LEN];      /* 2.13 format */
        fxpt_16         PresentSpeech[F_LEN];           /* 15.0 format */
        fxpt_16         scale;                          /* 3.12 format */
        static fxpt_16  AdaptiveCB[ACB_SIZE];           /* 15.0 format */
        fxpt_16         OptimumExcitation[SF_LEN];      /* 15.0 format */

FXPT_PUSHSTATE("Analysis", -1.0, -1.0);

        /* High Pass Filter Input Speech */
        HPF_InSpeech(SpeechIn);

        /* LP Analysis of Input speech */
        LP_Analysis(SpeechIn, parameters->lsf);

        /* Interpolate LSPs */
        Interpolate(parameters->lsf, LSFint, ANALYSIS);

DUMPARR(LSFint[0],15,ORDER)
DUMPARR(LSFint[1],15,ORDER)
DUMPARR(LSFint[2],15,ORDER)
DUMPARR(LSFint[3],15,ORDER)

        /* Create vector of "present" speech for codebook searches */
        CreateSubFrame(SpeechIn, PresentSpeech);

DUMPARR0(PresentSpeech,F_LEN)

        /* Loop through subframes to find codebook indices */
        for (sf = 0; sf < NUM_SF; sf++) {

                /* Convert quantized, interpolated LSFs to PCs */
                LSFtoPC(LSFint[sf], pc);

DUMPARR(pc,13,ORDER)

                /* Find the residual from LP analysis using the quantized,
                 * interpolated LSFs
                 */
                FindLPResidual(&PresentSpeech[sf*SF_LEN], pc, LP_res);

DUMPARR0(LP_res,RES_LEN)

                /* Find Impulse Response response of PCs */
                FindImpulseResponse(pc, LPImpulseResponse);

DUMPARR(LPImpulseResponse,13,SF_LEN)

                /* Use the LP residual to find the correct adaptive
                 * codebook index and gain
                 */
                AdaptiveAnalysis(&PresentSpeech[sf*SF_LEN], pc,
                    LPImpulseResponse, AdaptiveCB, LP_res,
                    &parameters->AdaptiveDelay[sf],
                    &parameters->AdaptiveGain[sf], Adaptive_res);
/* printf("AdaptiveGain = %f\n", parameters->AdaptiveGain[sf] / 16384.0); */
LOGFIXED(7,parameters->AdaptiveDelay[sf]);
LOGFIXED(14,parameters->AdaptiveGain[sf]);

DUMPARR0(Adaptive_res,RES_LEN)
DUMPARR0(AdaptiveCB,ACB_SIZE)

                /* Calculate the appropriate scaling for the Stochastic
                 * Analysis
                 */
                ModifiedExcitation(LP_res, Adaptive_res, &scale);
LOGFIXED(12,scale);
DUMPARR0(Adaptive_res,RES_LEN)

                /* Calculate the correct stochastic codebook index and gain */
                StochasticAnalysis(scale, LPImpulseResponse, Adaptive_res,
                    &parameters->StochasticIndex[sf],
                    &parameters->StochasticGain[sf], OptimumExcitation);
/* printf("StochasticGain = %f\n", parameters->StochasticGain[sf] / 16.0); */

                /* Update adaptive codebook and residual filters for next
                 * subframe
                 */
                UpdateEverything(OptimumExcitation, &PresentSpeech[sf*SF_LEN],
                    AdaptiveCB, parameters->AdaptiveGain[sf],
                    parameters->AdaptiveDelay[sf], pc);
        }

        /*ShowData(&fpLook2, "ACB_gain.txt", parameters->AdaptiveGain, FLOAT2ASCII, NUM_SF);
        ShowData(&fpLook3, "ACB_delay.txt", parameters->AdaptiveDelay, FLOAT2ASCII, NUM_SF);
        ShowData(&fpLook4, "SCB_index.txt", parameters->StochasticIndex, INT2ASCII, NUM_SF);
        ShowData(&fpLook5, "SCB_gain.txt", parameters->StochasticGain, FLOAT2ASCII, NUM_SF);*/
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               CreateSubFrame
*
* FUNCTION
*               Create subframe vector from past and current samples
* SYNOPSIS
*               CreateSubFrame(FutureSpeech, PresentSpeech)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       FutureSpeech    fxpt_16  i      Frame of future speech
*       PresentSpeech   fxpt_16  o      Frame of present speech
*
**************************************************************************/

void CreateSubFrame(
fxpt_16 FutureSpeech[F_LEN],
fxpt_16 PresentSpeech[F_LEN])
{
        static fxpt_16  PastSpeech[F_LEN/2];
        static int      first=TRUE;
        //int           i;

FXPT_PUSHSTATE("CreateSubFrame", -1.0, -1.0);

        /* Initialize past speech on first pass through */
        if (first) {
                first = FALSE;
                ZeroArray16 (PastSpeech, F_LEN/2);
                //for (i=0; i<F_LEN/2; i++)
                //      PastSpeech[i] = 0;
        }

        /* Create vector of present speech from past and future speech */
        MoveArray16 (PresentSpeech, PastSpeech, F_LEN/2);
        MoveArray16 (PresentSpeech + F_LEN/2, FutureSpeech, F_LEN/2);
        // for (i=0; i<F_LEN/2; i++) {
        //      PresentSpeech[i] = PastSpeech[i];
        //      PresentSpeech[i+F_LEN/2] = FutureSpeech[i];
        // }

        /* Save remaining future speech for past speech in the next frame */
        MoveArray16 (PastSpeech, FutureSpeech + F_LEN/2, F_LEN/2);
        // for (i=0; i<F_LEN/2; i++) {
        //      PastSpeech[i] = FutureSpeech[i+F_LEN/2];
        // }

        /* I suppose the 2 previous loops could be combined for better speed,
         * but 2 separate loops may be easier to understand ...
         */
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               FindImpulseResponse
*
* FUNCTION
*               Compute impulse response with direct form filter
* SYNOPSIS
*               FindImpulseResponse(pc, h)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       pc              fxpt_16  i      Frame of Predictor Coefficients
*       h               fxpt_16  o      Impulse response
*
**************************************************************************/

void FindImpulseResponse(
fxpt_16 pc[ORDER+1],                    /* 2.13 format */
fxpt_16 h[SF_LEN])                      /* 2.13 format */
{
        fxpt_16 pcexp[ORDER+1];

FXPT_PUSHSTATE("FindImpulseResponse", -1.0, -1.0);
        /* Initialize impulse response array */
        ZeroArray16(h, SF_LEN);
        h[0] = 8192;

        /* Bandwidth expand input predictor coefficients */
        BWExpand(GAMMA, pc, pcexp);

        /* Filter h with expanded PCs (no past history here) */
        FilterImpulseResponse(h, pcexp);
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               FindLPResidual
*
* FUNCTION
*               Compute residual from lpc systhesis
* SYNOPSIS
*               FindLPResidual(pc, residual)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       speech          fxpt_16  i      Subframe of input speech
*       pc              fxpt_16  i      Frame of Predictor Coefficients
*       residual        fxpt_16  o      Residual from LP analysis
*
**************************************************************************/

void FindLPResidual(
fxpt_16 speech[SF_LEN],                 /* 15.0 format */
fxpt_16 pc[ORDER+1],                    /* 2.13 format */
fxpt_16 residual[RES_LEN])              /* 15.0 format */
{
        fxpt_32 br[RES_LEN];    /* big residual, 16.15 format */
        fxpt_16 pcexp[ORDER+1];
        int     i;

FXPT_PUSHSTATE("FindLPResidual", -1.0, -1.0);
        /* Calculate zero input response  */
        ZeroArray16(residual, RES_LEN);

DUMPARR(pc,13,ORDER+1)

DUMPARR(LP_ResP.memory,15,LP_ResP.order)

		do_pfilt_dynamic(&LP_ResP, pc, residual);

DUMPARR(LP_ResP.memory,15,LP_ResP.order)

DUMPARR0(residual,RES_LEN)

        /* Calculate diff between zero input response and original speech */
        for (i=0; i<RES_LEN; i++) {
                br[i] = fxpt_sub32(
                    fxpt_shr32_fast(fxpt_deposit_h(speech[i]), 1),
                    fxpt_shr32_fast(fxpt_deposit_h(residual[i]), 1));
        }

DUMPARR(br,15,RES_LEN)

        /* Perceptual Weighting of the difference */
        BWExpand(GAMMA, pc, pcexp);

DUMPARR(pcexp,13,ORDER+1)

        do_zpfilt_dynamic32to16(&LP_ResZ, &LP_ResP2, pc, pcexp, br, residual);
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               UpdateEverything
*
* FUNCTION
*               Update adaptive parameters for next subframe
* SYNOPSIS
*               UpdateEverything(OptExcVec, Speech AdaptCB, AdaptGain, AdaptDelay, pc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       OptExcVec       fxpt_16  i      Optimial excitation vector from
*                                       stochastic analysis
*       Speech          fxpt_16  i      Subframe of present speech
*       AdaptCB         fxpt_16 i/o     Adaptive codebook vector
*       AdaptGain       fxpt_16  i      Adaptive codebook gain
*       AdaptDelay      fxpt_16  i      Adaptive codebook delay
*       pc              fxpt_16  i      Subframe of interpolated predictor
*                                       coefficients
*
**************************************************************************/

void UpdateEverything(
fxpt_16 OptExcVec[SF_LEN],              /* 15.0 format */
fxpt_16 Speech[SF_LEN],                 /* 15.0 format */
fxpt_16 AdaptCB[ACB_SIZE],              /* 15.0 format */
fxpt_16 AdaptGain,                      /* 1.14 format */
fxpt_16 AdaptDelay,                     /* 8.7 format */
fxpt_16 pc[ORDER+1])                    /* 2.13 format */
{
        fxpt_16         pcexp[ORDER+1];
        int             i;
        static int      first=TRUE;

FXPT_PUSHSTATE("UpdateEverything", -1.0, -1.0);
        /* Initialization */
        if (first) {
                first = FALSE;
                for (i=0; i<ACB_SIZE; i++)
                        AdaptCB[i] = 0;
        }

        /* Update Adaptive Codebook vector */
        ConstructAdaptCW(OptExcVec, AdaptCB, AdaptGain, AdaptDelay);

        /* Update Residuals */
        /* LP Filter the optimal excitation vector */
        do_pfilt_dynamic(&Update_ResP, pc, OptExcVec);

        /* Calculate Residual */
        for (i=0; i<RES_LEN; i++) {
                OptExcVec[i] = fxpt_sub16(Speech[i], OptExcVec[i]);
        }

        /* Perceptual Weighting of residual */
        BWExpand(GAMMA, pc, pcexp);
        do_zpfilt_dynamic(&Update_ResZ, &Update_ResP2, pc, pcexp, OptExcVec);

        /* Assign updated memories to residual filters */
        LP_ResZ = Update_ResZ;
        LP_ResP = Update_ResP;
        LP_ResP2 = Update_ResP2;

        Adapt_ResZ = Update_ResZ;
        Adapt_ResP = Update_ResP;
        Adapt_ResP2 = Update_ResP2;
FXPT_POPSTATE();
}
